﻿using System.Collections;
using System.Text;
using SimpleJSON;
using ROSBridgeLib.geometry_msgs;
using UnityEngine;

namespace ROSBridgeLib {
    namespace geometry_msgs {
        public class Vector4Msg : ROSBridgeMsg {
            private double _x;
            private double _y;
            private double _z;
            private double _w;


            public Vector4Msg() {}
            public Vector4Msg(JSONNode msg) {
                _x = double.Parse(msg["x"]);
                _y = double.Parse(msg["y"]);
                _z = double.Parse(msg["z"]);
                _w = double.Parse(msg["w"]);
            }

            public Vector4Msg(double x, double y, double z,double w) {
                _x = x;
                _y = y;
                _z = z;
                _w = w;
            }

            public static string GetMessageType() {
                return "geometry_msgs/Vector4";
            }

            public double GetX() {
                return _x;
            }

            public double GetY() {
                return _y;
            }

            public double GetZ() {
                return _z;
            }

            public double GetW()
            {
                return _w;
            }

            public override string ToString() {
                return "Vector4 [x=" + _x + ",  y="+ _y + ",  z=" + _z + ", w="+_w+"]";
            }

            public override string ToYAMLString() {
                return "{\"x\" : " + _x + ", \"y\" : " + _y + ", \"z\" : " + _z + ", \"w\" : " + _w+ "}";
            }


            public Vector4 ToVector4()
            {

                return new Vector4((float)_x,(float)_y,(float)_z,(float)_w);

            }


            public void FromVector4(Vector4 i)
            {

                _x=i.x;
                _y=i.y;
                _z=i.z;
                _w=i.w;

            }

            public void FromQuaternion(Quaternion q)
            {
                _x=q.x;
                _y=q.y;
                _z=q.z;
                _w=q.w;


            }

            public Quaternion ToQuaternion()
            {

                return new Quaternion((float)_x,(float)_y,(float)_z,(float)_w);

            }
        }
    }
}